#include "MagnetInMotion.h"
#include "GlutStuff.h"
#include "btBulletDynamicsCommon.h"
#include "GLDebugDrawer.h"

///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 1.

bool dd=true;

static GLDebugDrawer gDebugDraw;

void MagnetInMotion::DrawMField(const btVector3& pos) {
	glDisable (GL_DEPTH_TEST);
	btVector3 m_sample = Magnet::CalcMField(NULL, pos);
#ifdef _DEBUG/*
	printf("MField at ");
	PrintVector3(pos);
	printf(" : ");
	PrintVector3(m_sample);
	printf("\n");*/
#endif

	const float max_l = 2.5;

	m_sample/=800.;
	float l = m_sample.length();

	if (l>max_l) { 
		m_sample.normalize();
		m_sample *= max_l;
	}
	m_sample*=0.5;
	DrawArrow(pos, pos + m_sample, btVector3(0, 0, 1));

	DrawArrow(pos + m_sample, pos + m_sample*2, btVector3(1,0,0));
	glEnable (GL_DEPTH_TEST);
}

void MagnetInMotion::clientMoveAndDisplay()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

	//simple dynamics world doesn't handle fixed-time-stepping
	float ms = getDeltaTimeMicroseconds();
	
	///step the simulation
	if (m_dynamicsWorld)
	{
		m_dynamicsWorld->stepSimulation(ms / 1000000.f);
		Magnet::Simulation();
	//	fparticleSystem->Simulation(0.0001f);
		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();
	}
	
	// draw magnet field 
	// very slow - turn on in release mode
#ifdef _DEBUG/*
	for (int x=-20; x<21; x++) {
		for (int z=-20; z<21; z++) {
			DrawMField(btVector3(x*.5, 1, z*.5));
		}
	}*/
#endif

	renderme();
	particleSystem->renderParticle(m_drawline);
	/*
	if (m_drawline) {
		for (int x=-15; x<16; x++) {
			for (int z=-15; z<16; z++) {
				DrawMField(btVector3(x*0.5,0,z*0.5));
			}
		}
	}
	*/
#ifdef _DEBUG
	if (m_drawline) {
		Magnet::DrawMagnetsLine();
	}
#endif
	glFlush();

	swapBuffers();

}


void MagnetInMotion::displayCallback(void) {

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	
	renderme();

	//optional but useful: debug drawing to detect problems
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();

	glFlush();
	swapBuffers();
}


void MagnetInMotion::initPhysics() {
	
	dd = !dd;
	setTexturing(true);
	setShadows(true);

	setCameraDistance(btScalar(SCALING*5.));

	///collision configuration contains default setup for memory, collision setup
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	//m_collisionConfiguration->setConvexConvexMultipointIterations();

	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);

	m_broadphase = new btDbvtBroadphase();

	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;

	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
	m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
	
	m_dynamicsWorld->setGravity(btVector3(0,-9.8,0));


	btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(150.),btScalar(50.),btScalar(150.)));
	//groundShape->initializePolyhedralFeatures();
	//btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
	
	m_collisionShapes.push_back(groundShape);

	btTransform groundTransform;
	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0,-50,0));

	//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
	{
		btScalar mass(0.);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0,0,0);
		if (isDynamic)
			groundShape->calculateLocalInertia(mass,localInertia);

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);

		//add the body to the dynamics world
		m_dynamicsWorld->addRigidBody(body);

	}
	/*
	btVector3 localInertia(0,0,0);
	btScalar mass(0.01);
	btBoxShape* bx = new btBoxShape(btVector3(.2, .2, .2));
	
	
	for (int i=0;i<100;i++) {
		for (int j=0;j<100;j++) {

				btTransform tf;
				tf.setIdentity();
				tf.setOrigin(btVector3(i/4., 10., j/4.));
				btDefaultMotionState* myMotionState = new btDefaultMotionState(tf);

				//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
				
				btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,bx,localInertia);
				btRigidBody* body = new btRigidBody(rbInfo);

				//add the body to the dynamics world
				m_dynamicsWorld->addRigidBody(body);

			
		}
	}*/
	
	Magnet::SetDynamicsWorld(m_dynamicsWorld, m_collisionShapes);
/*	if (dd)
		fparticleSystem = new FParticleSystem(20000);
	else
		fparticleSystem = new FParticleSystem(100);
*/	particleSystem->SetDynamicsWorld(m_dynamicsWorld);
	
	// create magnet
	
	Magnet* mg = new Magnet(8);
	mg->SetMass(0);
	mg->SetPosition(btVector3(4, -0.6, 4));
	mg->SetScale(btVector3(2, 0.2, 0.5));
	mg->SetFerroMagnet(false);
	mg->SetMagnetization(btVector3(600, 0., 0.));
	mg->RegisterToWorld();
	/*
	Magnet* mg2 = new Magnet(8);
	mg2->SetMass(6);
	mg2->SetPosition(btVector3(-5, 10, 0));
	mg2->SetScale(btVector3(2, 0.15, 0.4));
	mg2->SetFerroMagnet(false);
	mg2->SetMagnetization(btVector3(1, 0., 0.));
	mg2->RegisterToWorld();
	*//*
	Magnet* mg3 = new Magnet(4);
	mg3->SetMass(0.01);
	mg3->SetPosition(btVector3(4., 1, 4));
	mg3->SetScale(btVector3(0.2, 0.2, 0.2));
	mg3->SetFerroMagnet(true);
	mg3->SetPermiability(1000);
	mg3->RegisterToWorld();
	*/
	/*
	Magnet* mg4 = new Magnet(4);
	mg4->SetMass(.01);
	mg4->SetPosition(btVector3(0., 2., 1));
	mg4->SetScale(btVector3(0.2, 0.2, 0.2));
	mg4->SetFerroMagnet(true);
	mg4->SetPermiability(1000);
	mg4->RegisterToWorld();

	Magnet* mg5 = new Magnet(4);
	mg5->SetMass(.01);
	mg5->SetPosition(btVector3(-1.5, 2., 1));
	mg5->SetScale(btVector3(0.2, 0.2, 0.2));
	mg5->SetFerroMagnet(true);
	mg5->SetPermiability(1000);
	mg5->RegisterToWorld();*/
}



void	MagnetInMotion::clientResetScene()
{
	exitPhysics();
	initPhysics();
}


void	MagnetInMotion::exitPhysics()
{

	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	m_collisionShapes.clear();

	delete m_dynamicsWorld;
	
	delete m_solver;
	
	delete m_broadphase;
	
	delete m_dispatcher;

	delete m_collisionConfiguration;

	Magnet::DeleteAllMagnet();

	delete particleSystem;
}